/********************************************************************
*  Sang Phuoc Mai - maiphuocsang@gmail.com
*  Dec - 08 - 2014
*  Feetech Smart Servo Control
*
*  Version 1.0
*
*  File name: FeetechSCServo.h
*
*  References:
*   1. Dynamixel library: https://code.google.com/p/slide-33/downloads/list
*   2. Feetech page: http://www.rcservozone.com/Wiki.jsp?page=About%20SCServo
*
*    
*********************************************************************/

#ifndef _FeetechSCServo_h_
#define _FeetechSCServo_h_

//#########################################################################
//################ define - Feetech Hex code table ######################
// EEPROM AREA
#define EEPROM_ID                       0x05
#define EEPROM_MIN_ANGLE_LIMIT_L         0x09
#define EEPROM_MAX_TORQUE_L             0x10
#define EEPROM_BAUD_RATE                0x06
#define EEPROM_ROM_LOCK					0x25
#define EEPROM_GOAL_SPEED_L				0x23

// RAM AREA
#define RAM_TORQUE_ENABLE            	0x1F
#define RAM_PUNCH_L                  	0x18
#define RAM_PRESENT_POSITION_L       	0x29
#define RAM_CW_COMPLIANCE_MARGIN		0x1A
#define RAM_PROPORTIONAL_GAIN 			0x15
#define RAM_PRESENT_VOLTAGE          	0x2F
#define RAM_PRESENT_TEMPERATURE			0x30



//#########################################################################
//################ Instruction commands Set ###############################
#define CMD_PING                	0x01
#define CMD_READ_DATA          		0x02
#define CMD_WRITE_DATA          	0x03
#define CMD_REG_WRITE_DATA      	0x04
#define CMD_ACTION              	0x05
#define CMD_RESET               	0x06
#define CMD_SYNC_WRITE          	0x83

//#########################################################################
//################ Instruction packet lengths #############################
#define RESET_LENGTH							0x02
#define ENABLE_TORQUE_LENGTH              		0x04
#define SERVO_WRITE_DEFAULT_LENGTH				0x07
#define SERVO_ACTION_LENGTH						0x07
#define READ_POS_LENGTH 						0x04
#define READ_ONE_BYTE_LENGTH        	0x01
#define READ_TWO_BYTE_LENGTH        			0x02
#define SET_ID_LENGTH              				0x04
#define	SET_MODE_LENGTH							0x07
#define SET_MAX_TORQUE_LENGTH			0x05
#define SET_PUNCH_LENGTH				0x05
#define SET_BD_LENGTH              		0x04
#define SET_COMP_MARGIN					0x05
#define PING_LENGTH						0x02
#define LOCK_ROM_LENGTH 				0x04
#define SET_PID_LENGTH					0x06
#define SET_SPEED_LENGTH				0x05
#define READ_VOLT_LENGTH            	0x04
#define READ_TEMP_LENGTH            	0x04

//#########################################################################
//############################ Specials ###################################
#define HEADER                     0xFF

#define		B_1M		0x00
#define		B_0_5M		0x01
#define		B_250K		0x02
#define		B_128K		0x03
#define		B_115200	0x04
#define		B_76800		0x05
#define		B_57600		0x06
#define		B_38400		0x07

#define		LOCK        1
#define		UNLOCK      0

#define	STATUS_PACKET_TIMEOUT			50		// in millis()
#define STATUS_FRAME_BUFFER				5
class FeetechClass {
private:
	void transmitInstructionPacket(void);
	unsigned int readStatusPacket(void);
    
public:
	unsigned int servoPing(unsigned char);
	unsigned int enableTorque(unsigned char, bool);
	unsigned int servoAction(unsigned char, unsigned int, unsigned int );
	unsigned int resetDefaultPosition(unsigned char);

	unsigned int setDefaultPosition(unsigned char, unsigned int, unsigned int );
	unsigned int setID(unsigned char, unsigned char);
	unsigned int setModeAndLimitAngle(unsigned char, bool, unsigned int, unsigned int );
	unsigned int setLimitTorque(unsigned char, int);
	unsigned int setPunch(unsigned char, unsigned int );
	unsigned int setBaudRate(unsigned char, long);
	unsigned int setComplianceMargin(unsigned char, unsigned int, unsigned int);
	unsigned int lockEprom(unsigned char, bool);
	unsigned int setPID(unsigned char, unsigned char, unsigned char, unsigned char);
	unsigned int setSpeed(unsigned char, unsigned int);
	unsigned int setLimitTemperature(unsigned char, unsigned char);
	unsigned int setLimitVoltage(unsigned char, unsigned int);

	unsigned int readPosition(unsigned char);
	unsigned int readVoltage(unsigned char);
	unsigned int readTemperature(unsigned char);
	unsigned int readLoad(unsigned char);
	unsigned int readSpeed(unsigned char);
	unsigned int readLockStatus(unsigned char);

struct IntructionPackage
{
	unsigned char servoID;
	unsigned char cmd_length;
	unsigned char cmd_action; //read, write, reset, sync ...
	unsigned char register_add;
	unsigned char first_param_low;
	unsigned char first_param_high;
	unsigned char second_param_low;
	unsigned char second_param_high;
	unsigned char third_param_low;
	unsigned char third_param_high;
	unsigned char checksum;

};

}